//
// Ephi - simulation of magnetic fields and particles
// Copyright (C) 2007 Indrek Mandre <indrek(at)mare.ee>
// For more information please see http://www.mare.ee/indrek/
//
// This program is free software; you can redistribute it and/or modify
// it under the terms of the GNU General Public License as published by
// the Free Software Foundation; either version 2 of the License, or
// (at your option) any later version.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU General Public License along
// with this program; if not, write to the Free Software Foundation, Inc.,
// 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
//

#include <math.h>
#include <assert.h>

#include "ephi.hpp"
#include "potential.hpp"

#include <set>

//#define PDIFF 0.009
//#define PDIFF 0.0115 // for polywell 15cm radius
//#define PDIFF 0.0090 // for 0.25m radius sphere
//#define PDIFF 0.009 // for 0.25m-0.075 radius torus
//#define PDIFF 0.018 // for polyp runs
//#define PDIFF 0.032 // for testing

#define MARGIN 3e-16

#define PCONST (SPEED_OF_LIGHT * SPEED_OF_LIGHT * MAGNETIC_CONSTANT)

PField::PField (emethod_t method, prec_t resolution) : method(method), resolution(resolution)
{
  statics = 0;
  pc_q = 0;
  pc_radius = 0;
  pc_pos.clear();
  pcounter = 0;
  added_charge = 0;
  cache = 0;
  no_cdist = false;
  transforms.push_back (transf3d (1, 0, 0, 0, 1, 0, 0, 0, 1));
}

PField::~PField ()
{
}

void PField::setPointCharge (const vect3d& pos, prec_t q)
{
  setUniformCharge (pos, q, 0.0);
}

void PField::setUniformCharge (const vect3d& pos, prec_t q, prec_t radius)
{
  pc_q = q;
  pc_pos = pos;
  pc_radius = radius;
  pc_rr = radius * radius;
  if ( radius != 0.0 )
      pc_rho = 3 * q / (4.0 * PREC_PI * radius * radius * radius);
}

PPoint& PField::addPoint (const vect3d& pos, const vect3d& ppos, prec_t potential)
{
  points.push_back (PPoint(pos, ppos, potential));
  points.back().id = pcounter++;
  if ( no_cdist )
      points.back().flags |= PPoint::NO_CDIST;
  return points.back();
}

void PField::addFeedPoint (PPoint& p, prec_t potential)
{
  if ( potential == UNKNOWN_POTENTIAL )
      return;
  fpoint_t fp;
  fp.point = &p;
  fp.potential = potential;
  feeds.push_back (fp);
}

namespace
{
  struct pair_t {
    pair_t (PPoint *p, prec_t phi) : p(p), phi(phi) { }
    PPoint *p;
    prec_t phi;
  };
};

void PField::addSphere (const vect3d& pos, prec_t radius, prec_t potential, bool backwards)
{
#if 1
  // my geometric shapes knowldge really sux
  // the idea is that we make a icosahedron here and split it until we get to desired resolution
  prec_t phi = (1+prec_t_sqrt(5.0))*0.5; // golden rectangle ratio
  prec_t elen = 2 * radius / prec_t_sqrt (phi * prec_t_sqrt(5.0));

  prec_t potoff = resolution * 3;
  if ( potoff > radius * 0.2 )
      potoff = radius * 0.3;
  prec_t pratio = potoff/radius;
  if ( backwards )
      pratio = -pratio;

  std::vector<PPoint*> spoints;
  vect3d p;
  p = pos + vect3d(0, -1, -phi).normal() * radius; spoints.push_back (&addPoint(p, p + (pos - p) * pratio, potential));
  p = pos + vect3d(0, -1, phi).normal() * radius; spoints.push_back (&addPoint(p, p + (pos - p) * pratio, potential));
  p = pos + vect3d(0, 1, -phi).normal() * radius; spoints.push_back (&addPoint(p, p + (pos - p) * pratio, potential));
  p = pos + vect3d(0, 1, phi).normal() * radius; spoints.push_back (&addPoint(p, p + (pos - p) * pratio, potential));

  p = pos + vect3d(-1, -phi, 0).normal() * radius; spoints.push_back (&addPoint(p, p + (pos - p) * pratio, potential));
  p = pos + vect3d(-1, phi, 0).normal() * radius; spoints.push_back (&addPoint(p, p + (pos - p) * pratio, potential));
  p = pos + vect3d(1, -phi, 0).normal() * radius; spoints.push_back (&addPoint(p, p + (pos - p) * pratio, potential));
  p = pos + vect3d(1, phi, 0).normal() * radius; spoints.push_back (&addPoint(p, p + (pos - p) * pratio, potential));

  p = pos + vect3d(-phi, 0, -1).normal() * radius; spoints.push_back (&addPoint(p, p + (pos - p) * pratio, potential));
  p = pos + vect3d(-phi, 0, 1).normal() * radius; spoints.push_back (&addPoint(p, p + (pos - p) * pratio, potential));
  p = pos + vect3d(phi, 0, -1).normal() * radius; spoints.push_back (&addPoint(p, p + (pos - p) * pratio, potential));
  p = pos + vect3d(phi, 0, 1).normal() * radius; spoints.push_back (&addPoint(p, p + (pos - p) * pratio, potential));

  std::vector<PLink*> slinks;
  slinks.reserve(30);
  for ( size_t i = 0; i < spoints.size(); i++ )
    {
      addFeedPoint (*spoints[i], potential);
      for ( size_t j = 0; j < spoints.size(); j++ )
        {
          if ( 0.9 * (spoints[i]->pos - spoints[j]->pos).length() < elen )
            {
              PLink *l = safe_connect (*spoints[i], *spoints[j], slinks);
              if ( l ) slinks.push_back (l);
            }
        }
    }

  splitSphericalTriangles (pos, radius, spoints, slinks, potential, backwards, pratio);

#if 0
  // now we need to start splitting the icosahedron more liking to our size
  while ( elen > resolution )
    {
      // first create mid-points
      for ( size_t i = 0; i < slinks.size(); i++ )
        {
          PLink& l = *slinks[i];
          vect3d p = 0.5 * (l.a->pos + l.b->pos);
          p = pos + (p - pos).normal() * radius;
          l.tmp = &addPoint (p, p + (pos - p) * pratio, potential);
        }

      // find connections between midpoints
      std::vector<std::pair<PPoint*,PPoint*> > tc;
      tc.reserve (slinks.size() * 2);
      for ( size_t i = 0; i < spoints.size(); i++ )
        {
          PPoint *a = spoints[i];
          for ( PPoint::links_t::iterator alit = a->links.begin(); alit != a->links.end(); alit++ )
            {
              PLink *l1 = *alit;
              PPoint *b = l1->a == a ? l1->b : l1->a;
              for ( PPoint::links_t::iterator blit = b->links.begin(); blit != b->links.end(); blit++ )
                {
                  PLink *l2 = *blit;
                  if ( l1 == l2 ) continue;
                  PPoint *c = l2->a == b ? l2->b : l2->a;
                  for ( PPoint::links_t::iterator clit = c->links.begin(); clit != c->links.end(); clit++ )
                    {
                      PLink *l3 = *clit;
                      if ( l2 == l3 ) continue;
                      PPoint *d = l3->a == c ? l3->b : l3->a;
                      if ( d != a ) continue;
                      tc.push_back (std::make_pair(l1->tmp, l2->tmp));
                      tc.push_back (std::make_pair(l2->tmp, l3->tmp));
                      tc.push_back (std::make_pair(l3->tmp, l1->tmp));
                      // we have found a triangle now, formed between l1, l2 and l3
                    }
                }
            }
        }

      // connect midpoints
      std::vector<PLink*> new_slinks;
      new_slinks.reserve (slinks.size() * 4);
      for ( size_t i = 0; i < tc.size(); i++ )
        {
          PLink *l = safe_connect (*tc[i].first, *tc[i].second, new_slinks);
          if ( l ) new_slinks.push_back (l);
        }

      // finally split up old links
      spoints.reserve (spoints.size() * 4);
      for ( size_t i = 0; i < slinks.size(); i++ )
        {
          PLink&l = *slinks[i];
          spoints.push_back (l.tmp);
          new_slinks.push_back (&l);
          new_slinks.push_back (&split(l, *l.tmp));
        }
      slinks.swap (new_slinks);
      elen = elen * 0.5;
    }
#endif

  if (0)
  for ( size_t i = 0; i < spoints.size(); i++ )
      printf ("%.32g\n", prec2double ((spoints[i]->pos - pos).length()));
#else
  vect3d normal(1, 0, 0);

  prec_t angle = asin(prec2double(resolution/radius));

  size_t n = (size_t)ceil(PREC_PI / angle);
  if ( (n % 2) ) n++;
  prec_t a = PREC_PI / n;

  std::vector<pair_t> last_row;
  std::vector<pair_t> next_row;

  prec_t potoff = resolution * 3;
  if ( potoff > radius * 0.2 )
      potoff = radius * 0.3;

  for ( size_t i = 0; i <= n; i++ )
    {
      prec_t offset = radius * cos(a * i);
      vect3d c = pos - offset * normal;
      prec_t sr = prec_t_sqrt(radius * radius - offset * offset);

      prec_t circum = sr * 2 * PREC_PI;
      size_t l = (size_t)ceil(circum / resolution);
      if ( !l ) l = 1;
      prec_t aadd = 2 * PREC_PI / l;

      next_row.clear();
      next_row.reserve (l);

      prec_t arand = (i % 2) ? aadd * 0.5 : 0;
      arand = 0;

      for ( size_t j = 0; j < l; j++ )
        {
          prec_t y, z;
          prec_t phi = j * aadd + arand;
          y = cos(phi) * sr;
          z = sin(phi) * sr;

          vect3d pl = c + vect3d(0, y, z);
          PPoint* point = &addPoint (pl, pl - (pl - pos).normal() * potoff, potential);
          if ( i == 0 || i == n || n > 5 && i == n / 2 && !(j % 4) && j + 2 < l )
              addFeedPoint (*point, potential);
          if ( !next_row.empty() )
              connect (*point, *next_row.back().p);
          if ( j > 1 && j == l - 1 )
              connect (*point, *next_row[0].p);
          next_row.push_back (pair_t(point, phi));
        }

#if 1
      bool swap = true;
      //printf ("row\n");
      if ( !last_row.empty() )
        {
          if ( last_row.size() > next_row.size() )
            {
              last_row.swap (next_row);
              swap = false;
            }

          if ( last_row.size() != 1 )
            {
              //printf ("preconnect 0(%f) ->%zu(%f)\n", next_row[0].phi, last_row.size()-1, last_row[last_row.size()-1].phi);
              connect (*next_row[0].p, *last_row[last_row.size()-1].p);
            }
          //printf ("%u vs %u\n", next_row.size(), last_row.size());
          size_t k = 0;
          for ( size_t j = 0; j < next_row.size(); j++ )
            {
              //printf ("connect %zu(%f) ->%zu(%f)\n", j, next_row[j].phi, k, last_row[k].phi);
              connect (*next_row[j].p, *last_row[k].p);
              while ( next_row[j].phi > last_row[k].phi && last_row.size() != 1 )
                {
                  k++;
                  if ( k >= last_row.size() )
                      break;
                  //printf ("subconnect %zu(%f) ->%zu(%f)\n", j, next_row[j].phi, k, last_row[k].phi);
                  connect (*next_row[j].p, *last_row[k].p);
                }
            }
        }

      if ( swap )
#endif
          last_row.swap (next_row);
    }
#endif
}

void PField::addSphereSegment (prec_t radius, const vect3d& n1, const vect3d& n2,
     const vect3d& n3, prec_t potential, bool outside, size_t level)
{
  std::vector<PLink*> slinks;

  vect3d pos1 = n1 * radius;
  vect3d pos2 = n2 * radius;
  vect3d pos3 = n3 * radius;

  std::vector<PPoint*> spoints;

  prec_t pratio = (outside ? -0.25 : 0.25);
  vect3d mid(0, 0, 0);

  spoints.push_back (&addPoint (pos1, pos1 + pratio * (mid - pos1), potential));
  spoints.push_back (&addPoint (pos2, pos2 + pratio * (mid - pos2), potential));
  spoints.push_back (&addPoint (pos3, pos3 + pratio * (mid - pos3), potential));
  slinks.push_back (&connect (*spoints[0], *spoints[1]));
  slinks.push_back (&connect (*spoints[1], *spoints[2]));
  slinks.push_back (&connect (*spoints[0], *spoints[2]));

  splitSphericalTriangles (vect3d(0, 0, 0), radius, spoints, slinks, potential, outside, pratio);
}

void PField::splitSphericalTriangles (const vect3d& mid, prec_t radius,
    std::vector<PPoint*> spoints, std::vector<PLink*> slinks,
    prec_t potential, bool outside, prec_t pratio)
{
  assert (!spoints.empty() && !slinks.empty());

  if ( pratio == 0 )
      pratio = (outside ? -0.333 : 0.667);

  // find the minimum of edge length
  prec_t longest = 9e99;
  for ( size_t i = 0; i < slinks.size(); i++ )
    {
      PLink& l = *slinks[i];
      prec_t len = (l.a->pos - l.b->pos).length();
      if ( longest > len )
          longest = len;
    }

  while ( longest > resolution )
    {
      // create mid-points
      for ( size_t i = 0; i < slinks.size(); i++ )
        {
          PLink& l = *slinks[i];
          vect3d p = 0.5 * (l.a->pos + l.b->pos);
          p = mid + (p - mid).normal() * radius;
          l.tmp = &addPoint (p, p + (mid - p) * pratio, potential);
        }

      // find connections between midpoints
      std::vector<std::pair<PPoint*,PPoint*> > tc;
      tc.reserve (slinks.size() * 2);
      for ( size_t i = 0; i < spoints.size(); i++ )
        {
          PPoint *a = spoints[i];
          for ( PPoint::links_t::iterator alit = a->links.begin(); alit != a->links.end(); alit++ )
            {
              PLink *l1 = *alit;
              PPoint *b = l1->a == a ? l1->b : l1->a;
              for ( PPoint::links_t::iterator blit = b->links.begin(); blit != b->links.end(); blit++ )
                {
                  PLink *l2 = *blit;
                  if ( l1 == l2 ) continue;
                  PPoint *c = l2->a == b ? l2->b : l2->a;
                  for ( PPoint::links_t::iterator clit = c->links.begin(); clit != c->links.end(); clit++ )
                    {
                      PLink *l3 = *clit;
                      if ( l2 == l3 ) continue;
                      PPoint *d = l3->a == c ? l3->b : l3->a;
                      if ( d != a ) continue;
                      tc.push_back (std::make_pair(l1->tmp, l2->tmp));
                      tc.push_back (std::make_pair(l2->tmp, l3->tmp));
                      tc.push_back (std::make_pair(l3->tmp, l1->tmp));
                      // we have found a triangle now, formed between l1, l2 and l3
                    }
                }
            }
        }

      // connect midpoints
      std::vector<PLink*> new_slinks;
      new_slinks.reserve (slinks.size() * 4);
      for ( size_t i = 0; i < tc.size(); i++ )
        {
          PLink *l = safe_connect (*tc[i].first, *tc[i].second, new_slinks);
          if ( l ) new_slinks.push_back (l);
        }

      // finally split up old links
      spoints.reserve (spoints.size() * 4);
      for ( size_t i = 0; i < slinks.size(); i++ )
        {
          PLink&l = *slinks[i];
          spoints.push_back (l.tmp);
          new_slinks.push_back (&l);
          new_slinks.push_back (&split(l, *l.tmp));
        }
      slinks.swap (new_slinks);

      longest *= 0.5;
    }
}

void PField::addTorus (const vect3d& pos, const vect3d& n, prec_t radius, prec_t wr, prec_t potential)
{
  vect3d normal = n.normal();
  vect3d r;
  normal.across(r);
  r = r * radius;
  vect3d p0 = pos + r;
  vect3d point_r = normal * wr;

  prec_t potway = resolution * 3;
  if ( potway > 0.75 * wr )
      potway = 0.75 * wr;

  size_t segment_count = (size_t)ceil((radius + wr) * 2 * PREC_PI / (2 * resolution / 2));
  while ( segment_count%8 ) segment_count++;
  size_t point_count = (size_t)ceil(wr * 2 * PREC_PI / resolution);
  while ( point_count%4 ) point_count++;

  if ( method == LINESYSTEM && 0 )
    {
      segment_count /= 4;
      //point_count /= 2;
    }

  prec_t segment_angle = 2 * PREC_PI / segment_count;
  prec_t point_angle = 2 * PREC_PI / point_count;

  vect3d p0_pc = p0 - pos;
  vect3d nXp0_pc = normal % p0_pc;

  std::vector<PPoint*> first_row;
  std::vector<PPoint*> last_row;

  for ( size_t i = 0; i < segment_count; i++ )
    {
      prec_t phi = segment_angle * i;
      vect3d center = pos + cos(phi) * p0_pc + sin(phi) * nXp0_pc;
      vect3d slope = -sin(phi) * p0_pc + cos(phi) * nXp0_pc;

      {
        vect3d pos = center;
        vect3d normal = slope.normal();
        vect3d p0 = pos + point_r;
        vect3d p0_pc = p0 - pos;
        vect3d nXp0_pc = normal % p0_pc;

        std::vector<PPoint*> next_row;
        for ( size_t j = 0; j < point_count; j++ )
          {
            prec_t phi = point_angle * j;
            vect3d mid = pos + cos(phi) * p0_pc + sin(phi) * nXp0_pc;
            vect3d pot_mid = mid - (mid - pos).normal() * potway;
            PPoint &p = addPoint (mid, pot_mid, potential);
            if ( (method != LINESYSTEM || 0) && !next_row.empty() )
                unsafe_connect (*next_row.back(), p);
            next_row.push_back (&p);
            if ( !last_row.empty() )
                unsafe_connect (*last_row[j], *next_row[j]);
            if ( i == segment_count - 1 )
                unsafe_connect (*next_row[j], *first_row[j]);

            if ( j == point_count / 4 && i && !(i % 4) )
                addFeedPoint(p, potential);
          }
        if ( method != LINESYSTEM || 0 )
            unsafe_connect (*next_row.back(), *next_row[0]);

        if ( i == 0 )
            first_row = next_row;
        last_row.swap (next_row);
      }
    }
}

void PField::addStrip (const vect3d& p0, const vect3d& p1, size_t n)
{
  prec_t len = (p1 - p0).length();
  if ( !n )
      n = (size_t)ceil(len/resolution);
  prec_t hf = len / n;
  vect3d p01 = (p1 - p0).normal();

  PPoint *prev = 0;
  for ( size_t i = 0; i <= n; i++ )
    {
      PPoint *p = &addPoint (p0 + i * hf * p01);
      if ( prev )
          connect (*prev, *p);
      prev = p;
    }
}

void PField::addT (const vect3d& pa, const vect3d& pb, const vect3d& pc, const vect3d& pd)
{
  PPoint& a = addPoint (pa);
  PPoint& b = addPoint (pb);
  PPoint& c = addPoint (pc);
  PPoint& d = addPoint (pd);
  connect (a, b);
  connect (b, c);
  connect (b, d);
}

void PField::print ()
{
#if 1
  for ( points_t::iterator it = points.begin(); it != points.end(); it++ )
    {
      char buf2[128];
      it->pos.sprint (buf2);
      printf ("%3u: %s: q=%g, rho=%g, pot=%g\n", (unsigned int)it->id, buf2, prec2double(it->charge),
          prec2double(it->rho), prec2double (getPotential(it->pot_pos)));
    }
#endif

  prec_t totpot = 0, minpot = 9e99, maxpot = -9e99;
  for ( links_t::iterator it = links.begin(); it != links.end(); it++ )
    {
#if 0
      it->mid.print("link.mid");
#endif
      prec_t pot = getPotential(it->mid);
      if ( minpot > pot )
          minpot = pot;
      if ( maxpot < pot )
          maxpot = pot;
#if 0
      printf("pot = %g\n", getPotential(it->mid));
#endif
      totpot += pot;
    }
  printf ("pot.avg=%g, pot.min=%g, pot.max=%g\n", prec2double(totpot / links.size()), prec2double(minpot), prec2double(maxpot));
  printf ("Total points: %u, total links: %u\n", (unsigned int)points.size(), (unsigned int)links.size()); 
}

prec_t PField::getAveragePotential ()
{
  prec_t potadd = 0;
  for ( links_t::iterator it = links.begin(); it != links.end(); it++ )
      potadd += getPotential(it->mid);
  return potadd / links.size();
}

void PField::setPotential (prec_t potential)
{
  prec_t ratio = potential / getAveragePotential();
  for ( points_t::iterator it = points.begin(); it != points.end(); it++ )
      it->charge *= ratio;
  added_charge *= ratio;
}

namespace {
  const char *PFMAGIC = "pFm1";

  struct pfheader
  {
    char magic[4];
    unsigned int pcounter;
    unsigned int points;
    unsigned int links;
    unsigned int feeds;
    double added_charge;
    double pc_q;
    double pc_pos_x, pc_pos_y, pc_pos_z;
    double pc_radius;
  };

  struct pfpoint
  {
    unsigned int id;
    double pos_x, pos_y, pos_z;
    double pot_pos_x, pot_pos_y, pot_pos_z;
    double charge, rho;
    size_t flags;
  };

  struct pflink
  {
    unsigned int id1;
    unsigned int id2;
  };

  struct pffeed
  {
    unsigned int id;
    prec_t potential;
  };
}

void PField::save (const char *path)
{
  FILE *fp = fopen (path, "wb");
  if ( !fp )
    {
      Ephi::debug (Ephi::DEBUG_CRITICAL, "PField::save: unable to open %s for writing!\n", path);
      return;
    }

  pfheader h;
  memset (&h, 0, sizeof(h));
  memcpy (h.magic, PFMAGIC, 4);
  h.pcounter = pcounter;
  h.points = points.size();
  h.links = links.size();
  h.feeds = feeds.size();
  h.added_charge = prec2double (added_charge);
  h.pc_q = prec2double (pc_q);
  h.pc_pos_x = prec2double (pc_pos.x);
  h.pc_pos_y = prec2double (pc_pos.y);
  h.pc_pos_z = prec2double(pc_pos.z);
  h.pc_radius = prec2double (pc_radius);
  fwrite (&h, sizeof(h), 1, fp);

  for ( points_t::iterator it = points.begin(); it != points.end(); it++ )
    {
      pfpoint pfp;
      memset (&pfp, 0, sizeof(pfp));
      pfp.id = it->id;
      pfp.pos_x = prec2double(it->pos.x);
      pfp.pos_y = prec2double(it->pos.y);
      pfp.pos_z = prec2double(it->pos.z);
      pfp.pot_pos_x = prec2double(it->pot_pos.x);
      pfp.pot_pos_y = prec2double(it->pot_pos.y);
      pfp.pot_pos_z = prec2double(it->pot_pos.z);
      pfp.charge = prec2double(it->charge);
      pfp.rho = prec2double(it->rho);
      pfp.flags = it->flags;
      fwrite (&pfp, sizeof(pfp), 1, fp);
    }

  for ( links_t::iterator it = links.begin(); it != links.end(); it++ )
    {
      pflink pfl;
      memset (&pfl, 0, sizeof (pfl));
      pfl.id1 = it->a->id;
      pfl.id2 = it->b->id;
      fwrite (&pfl, sizeof(pfl), 1, fp);
    }

  for ( feeds_t::iterator it = feeds.begin(); it != feeds.end(); it++ )
    {
      pffeed pff;
      memset (&pff, 0, sizeof (pff));
      pff.id = it->point->id;
      pff.potential = prec2double (it->potential);
      fwrite (&pff, sizeof(pff), 1, fp);
    }

  FPWriter w(fp);
  w.write (planes.size());
  w.write (&planes[0], planes.size() * sizeof (vect3d));
  w.write (transforms.size());
  w.write (&transforms[0], transforms.size() * sizeof (transf3d));
}

bool PField::load (const char *path)
{
  links.clear();
  feeds.clear();
  points.clear();
  planes.clear();
  transforms.clear();
  pc_q = 0;

  FILE *fp = fopen (path, "rb");
  if ( !fp )
      return false;

  pfheader h;
  if ( !fread (&h, sizeof(h), 1, fp) )
    {
      Ephi::debug (Ephi::DEBUG_ERROR, "PField::load: unable to load the header\n");
      fclose (fp);
      return false;
    }

  if ( memcmp (h.magic, PFMAGIC, 4) )
    {
      Ephi::debug (Ephi::DEBUG_ERROR, "PField::load: invalid magic\n");
      fclose (fp);
      return false;
    }

  //h.points = points.size();
  //h.links = links.size();
  //h.feeds = feeds.size();

  pcounter = h.pcounter;
  added_charge = h.added_charge;
  setUniformCharge (vect3d(h.pc_pos_x, h.pc_pos_y, h.pc_pos_z), h.pc_q, h.pc_radius);

  std::map<size_t, PPoint*> phash;

  for ( size_t i = 0; i < h.points; i++ )
    {
      pfpoint pfp;
      if ( !fread (&pfp, sizeof(pfp), 1, fp) )
        {
          Ephi::debug (Ephi::DEBUG_ERROR, "PField::load: unable to load the point %u\n", (unsigned int)i);
          fclose (fp);
          return false;
        }
      vect3d pos(pfp.pos_x, pfp.pos_y, pfp.pos_z);
      vect3d pot_pos(pfp.pot_pos_x, pfp.pot_pos_y, pfp.pot_pos_z);
      points.push_back (PPoint(pos, pot_pos));
      points.back().id = pfp.id;
      points.back().charge = pfp.charge;
      points.back().rho = pfp.rho;
      points.back().flags = pfp.flags;
      phash[pfp.id] = &points.back();
    }

  for ( size_t i = 0; i < h.links; i++ )
    {
      pflink pfl;
      if ( !fread (&pfl, sizeof(pfl), 1, fp) )
        {
          Ephi::debug (Ephi::DEBUG_ERROR, "PField::load: unable to load the link %u\n", (unsigned int)i);
          fclose (fp);
          return false;
        }
      std::map<size_t,PPoint*>::iterator p1it = phash.find(pfl.id1);
      std::map<size_t,PPoint*>::iterator p2it = phash.find(pfl.id2);
      if ( p1it == phash.end() || p2it == phash.end() )
        {
          Ephi::debug (Ephi::DEBUG_ERROR, "PField::load: unable to find link points at %u\n", (unsigned int)i);
          fclose (fp);
          return false;
        }
      connect(*p1it->second, *p2it->second);
    }

  for ( size_t i = 0; i < h.feeds; i++ )
    {
      pffeed pff;
      if ( !fread (&pff, sizeof(pff), 1, fp) )
        {
          Ephi::debug (Ephi::DEBUG_ERROR, "PField::load: unable to load feed %u\n", (unsigned int)i);
          fclose (fp);
          return false;
        }
      std::map<size_t,PPoint*>::iterator fit = phash.find(pff.id);
      if ( fit == phash.end() )
        {
          fclose (fp);
          return false;
        }
      addFeedPoint (*fit->second, pff.potential);
    }

  FPReader r(fp);
  size_t pc;
  r.read (pc);
  planes.resize (pc);
  r.read (&planes[0], planes.size() * sizeof (vect3d));
  size_t tc;
  r.read (tc);
  transforms.resize (tc);
  r.read (&transforms[0], transforms.size() * sizeof (transf3d));

  reflect();

  return !r.had_errors();
}

#define PRINTMOD 10000

void PField::calc_efield (const links_t::iterator& begin, const links_t::iterator& end, const vect3d* cache)
{
  if ( cache )
    {
      for ( links_t::iterator it = begin; it != end; it++ )
        {
          PLink& c = *it;
          vect3d efield;
          efield.clear();
          for ( points_t::iterator jt = points.begin(); jt != points.end(); jt++ )
              efield += jt->charge * *cache++;

          if ( pc_q != 0 )
              efield += pc_q * *cache++;

          c.q = efield * c.cc_normal;
        }
    }
  else
    {
      for ( links_t::iterator it = begin; it != end; it++ )
        {
          PLink& c = *it;
          vect3d efield;
          getEfield (c.mid, c.efield);
          prec_t f = c.efield * c.ab_normal;
          c.q = 0.05 * f * c.rr / PCONST;
        }
    }
}

void PField::calc_linesystem (LinearSystem& ls, size_t part, size_t n)
{
  size_t pc = n / THREADS_CPUCOUNT;
  size_t si = part * pc;
  size_t ei = si + pc;
  if ( part == THREADS_CPUCOUNT - 1 )
      ei = n;

  points_t::iterator it = points.begin();
  points_t::iterator end = points.begin();
  std::advance (it, si);
  std::advance (end, ei);
  while ( it != end )
    {
      PPoint& point = *it;
      vect3d pos = point.pot_pos;

      prec_t *row = ls.getRow (point.id);

      for ( links_t::iterator jt = links.begin(); jt != links.end(); jt++ )
        {
          PLink& c = *jt;
          for ( size_t i = 0; i < c.reflections.size(); i++ )
            {
              vect3d p0 = c.reflections[i].a;
              vect3d p1 = c.reflections[i].b;
              vect3d ln = c.reflections[i].ab_normal;
              prec_t llen = c.len;

              size_t p0i, p1i;
              vect3d r0, r1;
              prec_t a, b;

              vect3d tv = pos - p0;
              prec_t tm = ln * tv;
              if ( tm <= 0 )
                {
                  p0i = c.a->id;
                  p1i = c.b->id;
                  r0 = tv;
                  r1 = pos - p1;
                  a = -tm;
                  b = -(ln * r1);
                }
              else
                {
                  p0i = c.b->id;
                  p1i = c.a->id;
                  r0 = pos - p1;
                  r1 = tv;
                  a = (ln * r0);
                  b = tm;
                }
              prec_t r0len = r0.length();
              prec_t r1len = r1.length();

              prec_t logv = prec_t_log ((b + r1len) / (a + r0len));
              prec_t f1 = a / llen;
              prec_t f2 = (r1len - r0len) / llen;

              prec_t a0 = (1 + f1) * logv - f2;
              prec_t a1 = -f1 * logv + f2;

              row[p0i] += a0;
              row[p1i] += a1;
            }
        }

      prec_t bv = point.potential / (SPEED_OF_LIGHT*SPEED_OF_LIGHT*MAGNETIC_CONSTANT);
      if ( pc_q != 0 )
        {
          prec_t ret = 0;
          prec_t rr = (pos - pc_pos).magnitude();
          if ( rr < pc_rr )
            {
              ret += pc_q / pc_radius;
              ret += 4.0 * PREC_PI * pc_rho * (pc_rr - rr) / 6.0;
            }
          else
            {
              ret += pc_q / prec_t_sqrt (rr);
            }
          bv -= ret;
        }
      row[n] = bv;
      it++;
    }
}

bool PField::equalize ()
{
  reflect();

  switch (method)
    {
      case POINTSYSTEM:
        return equalize_pointsystem();
        break;
      case CHARGEFLOW:
        return equalize_chargeflow();
        break;
      case LINESYSTEM:
        return equalize_linesystem();
        break;
      default:
        abort();
        break;
    }
  return false;
}

bool PField::equalize_chargeflow ()
{
  TaskManager tmgr;

  size_t cacheline_length = points.size() + (pc_q == 0.0 ? 0 : 1);

  size_t cs = sizeof (vect3d) * links.size() * cacheline_length;
  cache = (vect3d*)malloc (cs);
  if ( cache )
    {
      Ephi::debug (Ephi::DEBUG_INFO, "PField::equalize: allocated %lu bytes of memory for a cache\n", (unsigned long)cs);
      vect3d *p = cache;
      for ( links_t::iterator it = links.begin(); it != links.end(); it++ )
        {
          vect3d& pos = it->mid;
          for ( points_t::iterator jt = points.begin(); jt != points.end(); jt++ )
            {
              vect3d r = pos - jt->pos;
              prec_t rr = r * r;
              prec_t m = rr * prec_t_sqrt(rr);
              *p++ = vect3d(r.x / m, r.y / m, r.z / m);
            }

          if ( pc_q != 0.0 )
            {
              vect3d r = pos - pc_pos;
              prec_t rr = r * r;
              prec_t m = rr * prec_t_sqrt(rr);
              *p++ = vect3d(r.x / m, r.y / m, r.z / m);
            }
        }
    }

  std::vector<task> tasks;
  tasks.resize (THREADS_CPUCOUNT);
  for ( size_t i = 0; i < THREADS_CPUCOUNT; i++ )
    {
      size_t pc = links.size() / THREADS_CPUCOUNT;
      tasks[i].pf = this;
      tasks[i].begin = links.begin();
      if ( cache )
          tasks[i].cache = cache + i * pc * cacheline_length;
      else
          tasks[i].cache = 0;
      std::advance (tasks[i].begin, i * pc);
      if ( i != THREADS_CPUCOUNT - 1 )
        {
          tasks[i].end = tasks[i].begin;
          std::advance (tasks[i].end, pc);
        }
      else
        {
          tasks[i].end = links.end();
        }
    }

  size_t n = 0;
  while (true)
    {
      for ( size_t i = 0; i < THREADS_CPUCOUNT; i++ )
          tmgr.addTask (&tasks[i]);
      while ( tmgr.run() );

      for ( feeds_t::iterator it = feeds.begin(); it != feeds.end(); it++ )
        {
          PPoint& p = *it->point;
          prec_t sum = 0, len = 0;
          for ( PPoint::links_t::iterator jt = p.links.begin(); jt != p.links.end(); jt++ )
            {
              sum += getPotential ((*jt)->mid);
              len += (*jt)->len;
            }
          sum /= p.links.size();
          len /= p.links.size() * 2;
          prec_t target = it->potential - sum;
          it->cp = sum;
          it->q = 0.1 * target * len / PCONST;
        }

      for ( feeds_t::iterator it = feeds.begin(); it != feeds.end(); it++ )
        {
          PPoint& p = *it->point;
          if ( !(n % PRINTMOD ) && 0 )
            {
              char buf[128];
              p.pos.sprint (buf);
              printf ("%s: potential = %f, added q = %g\n", buf, prec2double(it->cp), prec2double(it->q));
            }
          p.charge += it->q;
          added_charge += it->q;
        }
      
      for ( links_t::iterator it = links.begin(); it != links.end(); it++ )
        {
          PLink& l = *it;
#if 0
          if ( !(n % PRINTMOD ) )
            {
              char buf[128];
              l.efield.sprint (buf);
              printf ("%3zu-%3zu: %g %s\n", l.a->id, l.b->id, l.q, buf);
            }
#endif
          l.a->charge -= l.q;
          l.b->charge += l.q;
          l.q = 0;
        }

      if ( !(n % PRINTMOD ) && 0 )
          print();
      n++;
      if ( n == 40001 )
        break;
      if ( !(n % PRINTMOD) )
          if ( getchar() == 'q' )
              break;
      Ephi::debug (Ephi::DEBUG_INFO, "PField::equalize_chargeflow: %u/%u\n", (unsigned int)n, 300);
      //if ( n == 600 )
      if ( n == 300 )
          break;
    }

  if ( cache )
    {
      free (cache);
      cache = 0;
    }
//  print();
  return true;
}

void PField::calc_pointsystem (LinearSystem& ls, size_t part, size_t n)
{
  size_t pc = n / THREADS_CPUCOUNT;
  size_t si = part * pc;
  size_t ei = si + pc;
  if ( part == THREADS_CPUCOUNT - 1 )
      ei = n;

  points_t::iterator jt = points.begin();
  points_t::iterator end = points.begin();
  std::advance (jt, si);
  std::advance (end, ei);

  while ( jt != end )
    {
      vect3d pos = jt->pot_pos;
      prec_t b = jt->potential / (SPEED_OF_LIGHT*SPEED_OF_LIGHT*MAGNETIC_CONSTANT);
      if ( pc_q != 0 )
        {
          prec_t ret = 0;
          prec_t rr = (pos - pc_pos).magnitude();
          if ( rr < pc_rr )
            {
              ret += pc_q / pc_radius;
              ret += 4.0 * PREC_PI * pc_rho * (pc_rr - rr) / 6.0;
            }
          else
            {
              ret += pc_q / prec_t_sqrt (rr);
            }
          b -= ret;
          //b -= pc_q / (pos - pc_pos).length();
        }
      if ( statics != 0 )
        {
          prec_t pot = statics->getPotential (pos);
#if 0
          char buf[128];
          printf ("%s: pot=%g\n", pos.sprint(buf), pot);
#endif
          b -= pot / (SPEED_OF_LIGHT*SPEED_OF_LIGHT*MAGNETIC_CONSTANT);
        }
      prec_t *ptr = ls.getRow (jt->id);
      for ( points_t::iterator it = points.begin(); it != points.end(); it++ )
        {
          PPoint& p = *it;
          for ( size_t i = 0; i < p.reflections.size(); i++ )
              *ptr += 1.0 / (pos - p.reflections[i].pos).length();
          ptr++;
        }
      *ptr = b;
      jt++;
    }
}

bool PField::equalize_pointsystem ()
{
  size_t n = points.size();
  LinearSystem ls(n);
  ls.clear();

  TaskManager tmgr;
  std::vector<pointsystem_task> tasks;
  tasks.resize (THREADS_CPUCOUNT);
  for ( size_t i = 0; i < THREADS_CPUCOUNT; i++ )
    {
      tasks[i].pf = this;
      tasks[i].part = i;
      tasks[i].n = n;
      tasks[i].ls = &ls;
      tmgr.addTask (&tasks[i]);
    }
  while ( tmgr.run() );

  if ( !ls.solveGBS() )
      return false;

  added_charge = 0;
  const prec_t *result = ls.getResults();
  for ( points_t::iterator it = points.begin(); it != points.end(); it++ )
    {
      it->charge = *result++;
      added_charge += it->charge * it->reflections.size();
    }

#if 0
  for ( points_t::iterator it = points.begin(); it != points.end(); it++ )
      printf ("Point id=%u, q=%g, pot=%g\n", (unsigned int)it->id, prec2double(it->charge), prec2double(getPotential(it->pot_pos)));
#endif
  return true;
}

void PField::reflect ()
{
  // first off cut the planes
  if ( method == LINESYSTEM || method == POINTSYSTEM )
    {
      for ( size_t i = 0; i < planes.size(); i++ )
        {
          vect3d n = planes[i];
          // XXX this can be multi-threaded
          for ( links_t::iterator it = links.begin(); it != links.end(); )
            {
              PLink& l = *it;
              PPoint& a = *l.a;
              PPoint& b = *l.b;
              prec_t an = a.pos * n;
              prec_t bn = b.pos * n;
              if ( an <= -MARGIN && bn <= MARGIN || bn <= -MARGIN && an <= MARGIN )
                {
                  it = links.erase (it);
                  a.links.remove (&l);
                  b.links.remove (&l);
                  continue;
                }

              if ( method == POINTSYSTEM && (an <= -MARGIN || bn <= -MARGIN) )
                {
                  it = links.erase (it);
                  a.links.remove (&l);
                  b.links.remove (&l);
                  continue;
                }

              if ( an <= MARGIN && bn <= MARGIN )
                {
                  // boundary link, need to figure out what to with it one day
                  l.boundary = true;
                  it++;
                  continue;
                }

              if ( an <= -MARGIN || bn <= -MARGIN )
                {
                  vect3d ab = b.pos - a.pos;
                  prec_t t = -(a.pos * n) / (ab * n);
                  vect3d mid;
                  mid = a.pos + t * ab;
                  assert (mid*n<MARGIN && mid*n>-MARGIN);
                  PPoint& p = addPoint (mid, 0.5 * (a.pot_pos + b.pot_pos), 0.5 * (a.potential + b.potential));
                  if ( a.flags & PPoint::NO_CDIST && b.flags & PPoint::NO_CDIST )
                      p.flags |= PPoint::NO_CDIST;
                  split (l,p);
                  if ( an <= -MARGIN && (l.a == &a || l.b == &a) || bn <= -MARGIN && (l.a == &b || l.b == &b) )
                    {
                      l.a->links.remove (&l);
                      l.b->links.remove (&l);
                      it = links.erase (it);
                      continue;
                    }
                  else
                    {
                      PLink& bl = links.back();
                      bl.a->links.remove (&bl);
                      bl.b->links.remove (&bl);
                      links.pop_back();
                    }
                }

              it++;
            }

          // remove excess points, also renumber points
          pcounter = 0;
          for ( points_t::iterator it = points.begin(); it != points.end(); )
            {
              PPoint& p = *it;
              if ( method == LINESYSTEM )
                {
                  if ( p.links.empty() )
                    {
                      for ( feeds_t::iterator jt = feeds.begin(); jt != feeds.end(); jt++ )
                        {
                          if ( jt->point == &p )
                            {
                              feeds.erase (jt);
                              break;
                            }
                        }
                      it = points.erase (it);
                      continue;
                    }
                }
              else
                {
                  prec_t d = n * p.pos;
                  if ( d <= -MARGIN )
                    {
                      for ( feeds_t::iterator jt = feeds.begin(); jt != feeds.end(); jt++ )
                        {
                          if ( jt->point == &p )
                            {
                              feeds.erase (jt);
                              break;
                            }
                        }
                      it = points.erase (it);
                      continue;
                    }
                  if ( d <= MARGIN )
                      p.flags |= PPoint::IS_BOUNDARY;
                }
              it->id = pcounter++;
              it++;
            }
        }
    }

  // then do the reflections
  // XXX this can be multi-threaded
  for ( links_t::iterator it = links.begin(); it != links.end(); it++ )
    {
      PLink& c = *it;

      typedef std::list<std::pair<vect3d, vect3d> > duplicates_t;
      duplicates_t duplicates;
      PLink::reflections_t r;

      for ( size_t i = 0; i < transforms.size(); i++ )
        {
          vect3d p0 = transforms[i] * c.a->pos;
          vect3d p1 = transforms[i] * c.b->pos;
          vect3d ln = transforms[i] * c.ab_normal;

          if ( c.boundary )
            {
              duplicates_t::iterator kt;
              for ( kt = duplicates.begin(); kt != duplicates.end(); kt++ )
                {
                  if ( (p0 - kt->first).magnitude() < MARGIN * MARGIN &&
                      (p1 - kt->second).magnitude() < MARGIN * MARGIN )
                      break;
                }

              if ( kt != duplicates.end() )
                  continue;
              duplicates.push_back (std::make_pair (p0, p1));
            }
          r.push_back (PLink::reflection_t());
          r.back().a = p0;
          r.back().b = p1;
          r.back().ab_normal = ln;
        }
      c.reflections = r;
    }

  for ( points_t::iterator it = points.begin(); it != points.end(); it++ )
    {
      PPoint& p = *it;

      typedef std::list<vect3d> duplicates_t;
      duplicates_t duplicates;
      PPoint::reflections_t r;

      for ( size_t i = 0; i < transforms.size(); i++ )
        {
          vect3d pos = transforms[i] * p.pos;
          vect3d pot_pos = transforms[i] * p.pot_pos;

          if ( p.flags & PPoint::IS_BOUNDARY )
            {
              duplicates_t::iterator kt;
              for ( kt = duplicates.begin(); kt != duplicates.end(); kt++ )
                {
                  if ( (pos - *kt).magnitude() < MARGIN * MARGIN )
                      break;
                }

              if ( kt != duplicates.end() )
                  continue;
              duplicates.push_back (pos);
            }

          r.push_back (PPoint::reflection_t());
          r.back().pos = pos;
          r.back().pot_pos = pot_pos;
        }
      it->reflections = r;
    }
}

bool PField::equalize_linesystem ()
{
  size_t n = points.size();
  LinearSystem ls(n);
  ls.clear();

  TaskManager tmgr;
  std::vector<linesystem_task> tasks;
  tasks.resize (THREADS_CPUCOUNT);
  for ( size_t i = 0; i < THREADS_CPUCOUNT; i++ )
    {
      tasks[i].pf = this;
      tasks[i].part = i;
      tasks[i].n = n;
      tasks[i].ls = &ls;
      tmgr.addTask (&tasks[i]);
    }
  while ( tmgr.run() );

  if ( !ls.solveGBS() )
      return false;

  const prec_t *result = ls.getResults();
  for ( points_t::iterator it = points.begin(); it != points.end(); it++ )
      it->rho = *result++;

  for ( links_t::iterator lit = links.begin(); lit != links.end(); lit++ )
    {
      PLink& c = *lit;
      c.rhodiff_by_llen = (c.b->rho - c.a->rho) / c.len;
      added_charge += c.reflections.size() * (lit->len * lit->a->rho + 0.5 * lit->len * (lit->b->rho - lit->a->rho));
    }

  return true;
}

std::string PField::getPovrayScript ()
{
  char buf[1024];
  std::string ret;

  for ( points_t::iterator it = points.begin(); it != points.end(); it++ )
    {
      PPoint& p = *it;
      for ( size_t i = 0; i < p.reflections.size(); i++ )
        {
          vect3d pos = p.reflections[i].pos;
          vect3d pot_pos = p.reflections[i].pot_pos;

          //pos = p.pos;
          //pot_pos = p.pot_pos;
          sprintf (buf, "sphere { <%f, %f, %f>, %f pigment { rgb <0.8, 1.0, 0.8> } }\n",
              prec2double (pos.x), prec2double (pos.y), prec2double (pos.z),
              prec2double (resolution / 6));
          ret.append (buf);

          if (0)
          if ( method == POINTSYSTEM || method == LINESYSTEM )
            {
              sprintf (buf, "sphere { <%f, %f, %f>, %f pigment { rgb <0.0, 0.8, 0.0> } }\n",
                  prec2double (pot_pos.x), prec2double (pot_pos.y), prec2double (pot_pos.z),
                  prec2double (resolution / 3));
              ret.append (buf);
            }
          //break;
        }
    }
  
  for ( links_t::iterator jt = links.begin(); jt != links.end(); jt++ )
    {
      PLink& l = *jt;
      for ( size_t i = 0; i < l.reflections.size(); i++ )
        {
          vect3d pos1 = l.reflections[i].a;
          vect3d pos2 = l.reflections[i].b;
          //pos1 = l.a->pos;
          //pos2 = l.b->pos;
          sprintf (buf, "  cylinder { <%f, %f, %f>, <%f, %f, %f>, %f texture {StaticLineTexture}}\n",
              prec2double (pos1.x), prec2double (pos1.y), prec2double (pos1.z),
              prec2double (pos2.x), prec2double (pos2.y), prec2double (pos2.z),
              prec2double (resolution / 8));
          ret.append (buf);
          //break;
        }
    }

#if 0
  ret += "#declare PT = texture {pigment {rgbt <0, 1, 1, 0>} }";
  ret += "  difference { union { plane { <1, 0, 0>, 0 texture {PT} } plane { <0, 1, 0>, 0 texture {PT} } plane { <0, 0, 1>, 0 texture {PT} } plane { <0, 1, 1>, 0 texture {PT} } plane { <0, 1, -1>, 0 texture {PT} } plane { <1, 1, 0>, 0 texture {PT} } plane { <1, -1, 0>, 0 texture {PT} } plane { <1, 0, 1>, 0 texture {PT} } plane { <1, 0, -1>, 0 texture {PT} } } box { <-0.3, -0.3, -0.3>, <0.3, 0.3, 0.3> texture { pigment { rgbt<0, 0, 0, 1> }} inverse } } ";
#endif

  return ret;
}

std::string PField::getChargeConcentrationScript ()
{
  prec_t nmin = -9e99;
  prec_t nmax = 9e99;
  prec_t pmin = 9e99;
  prec_t pmax = -9e99;

  std::string ret;

  for ( points_t::iterator it = points.begin(); it != points.end(); it++ )
    {
      if ( it->flags & PPoint::NO_CDIST )
          continue;
      prec_t q = it->charge;
      if ( method == LINESYSTEM )
          q = it->rho;
      //printf ("%u: q=%g\n", (unsigned int)it->id, q);
      if ( q < 0 )
        {
          if ( q > nmin )
              nmin = q;
          if ( q < nmax )
              nmax = q;
        }
      else
        {
          if ( q < pmin )
              pmin = q;
          if ( q > pmax )
              pmax = q;
        }
    }

  vect3d positive(1, 0, 0);
  vect3d neutral(0, 0.5, 0);
  vect3d negative(0, 0, 1);

  nmin = pmin = 0;

  prec_t nm = nmin - nmax;
  prec_t pm = pmax - pmin;

  //printf ("nmin=%g, nmax=%g, nm=%g\n", nmin, nmax, nm);

  for ( points_t::iterator it = points.begin(); it != points.end(); it++ )
    {
      if ( it->flags & PPoint::NO_CDIST )
          continue;
      prec_t q = it->charge;
      if ( method == LINESYSTEM )
          q = it->rho;
      vect3d c;
      if ( q < 0 )
        {
          c = (q / nmax) * negative + (1 - q/nmax) * neutral;
          c = ((nmin - q) / nm) * negative + (1 - (nmin - q) / nm) * neutral;
          //char buf[128];
          //printf ("%g: %s\n", q, c.sprint(buf));
        }
      else
        {
          c = (q / pmax) * positive + (1 - q / pmax) * neutral;
          c = ((q - pmin) / pm) * positive + (1 - (q - pmin) / pm) * neutral;
        }

      for ( size_t i = 0; i < it->reflections.size(); i++ )
        {
          vect3d pos = it->reflections[i].pos;
          char buf[1024];
          sprintf (buf, "sphere { <%f, %f, %f>, %f pigment { rgb <%f, %f, %f> } }\n",
              prec2double (pos.x), prec2double (pos.y), prec2double (pos.z),
              prec2double (0.5 * resolution),
              prec2double (c.x), prec2double (c.y), prec2double (c.z));
          ret += buf;
        }
    }

  return ret;
}

void PField::getEfield (const vect3d& pos, vect3d& efield)
{
  efield.clear();

  if ( method == LINESYSTEM )
    {
      for ( links_t::iterator it = links.begin(); it != links.end(); it++ )
        {
          PLink& c = *it;
          for ( size_t i = 0; i < c.reflections.size(); i++ )
            {
              vect3d p0 = c.reflections[i].a;
              vect3d p1 = c.reflections[i].b;
              vect3d ln = c.reflections[i].ab_normal;
              prec_t rho0 = c.a->rho;
              prec_t rho1 = c.b->rho;
              prec_t rhodiff_by_llen = c.rhodiff_by_llen;

              vect3d r0, r1;
              prec_t a, b;
              prec_t u, v;
              vect3d n;

              vect3d tv = pos - p0;
              prec_t tm = ln * tv;
              if ( tm <= 0 )
                {
                  r0 = tv;
                  r1 = pos - p1;
                  a = -tm;
                  b = -(ln * r1);
                  u = rho0 - a * rhodiff_by_llen;
                  v = rhodiff_by_llen;
                  n = ln;
                }
              else
                {
                  r0 = pos - p1;
                  r1 = tv;
                  a = (ln * r0);
                  b = tm;
                  u = rho1 - a * -rhodiff_by_llen;
                  v = -rhodiff_by_llen;
                  n = -ln;
                }
              prec_t r0len = r0.length();
              prec_t r1len = r1.length();
              prec_t af = a + r0len;
              prec_t bf = b + r1len;
              prec_t lf = log(bf / af);
              vect3d r0n = r0 / r0len;
              vect3d r1n = r1 / r1len;
              vect3d add = v * (lf * n + r1n - r0n) + u * ((r1n - n) / bf - (r0n - n) / af);
              efield -= add;
            }
        }
    }
  else
    {
      // XXX this can be SSE-optimized
      for ( points_t::iterator it = points.begin(); it != points.end(); it++ )
        {
          PPoint& p = *it;
          for ( size_t i = 0; i < p.reflections.size(); i++ )
            {
              vect3d r = pos - p.reflections[i].pos;
              prec_t rr = r * r;
              efield += (it->charge / (rr * prec_t_sqrt(rr))) * r;
            }
        }
    }

  if ( pc_q != 0 )
    {
      vect3d r = pos - pc_pos;
      prec_t rr = r.magnitude();
      if ( rr < pc_rr )
        {
          efield += (pc_q / (pc_rr * pc_radius)) * r;
        }
      else
        {
          efield += (pc_q / (rr * prec_t_sqrt(rr))) * r;
        }
    }

  efield *= PCONST;
}

prec_t PField::getPotential (const vect3d& pos)
{
  prec_t ret = 0;

  if ( method == LINESYSTEM )
    {
      for ( links_t::iterator it = links.begin(); it != links.end(); it++ )
        {
          PLink& c = *it;
          for ( size_t i = 0; i < c.reflections.size(); i++ )
            {
              vect3d p0 = c.reflections[i].a;
              vect3d p1 = c.reflections[i].b;
              vect3d ln = c.reflections[i].ab_normal;

              prec_t rho0 = c.a->rho;
              prec_t rho1 = c.b->rho;

              prec_t rhodiff_by_llen = c.rhodiff_by_llen;

#if 0
              p0.print ("p0");
              p1.print ("p1");
              printf ("%g %g\n", rho0, rho1);
              ln.print ("ln");
              (p1 - p0).normal().print("p0p1.normal");
              printf ("rhodiff_by_llen: %g\n", rhodiff_by_llen);
#endif

              vect3d r0, r1;
              prec_t a, b;
              prec_t u, v;
              vect3d n;

              vect3d tv = pos - p0;
              prec_t tm = ln * tv;
              if ( tm <= 0 )
                {
                  r0 = tv;
                  r1 = pos - p1;
                  a = -tm;
                  b = -(ln * r1);
                  u = rho0 - a * rhodiff_by_llen;
                  v = rhodiff_by_llen;
                }
              else
                {
                  r0 = pos - p1;
                  r1 = tv;
                  a = (ln * r0);
                  b = tm;
                  u = rho1 - a * -rhodiff_by_llen;
                  v = -rhodiff_by_llen;
                }
              prec_t r0len = r0.length();
              prec_t r1len = r1.length();
              prec_t add = (u * prec_t_log ((b + r1len) / (a + r0len)) + v * (r1len - r0len));
              ret += add;
            }
        }
    }
  else
    {
      // XXX this can be SSE-optimized
      for ( points_t::iterator it = points.begin(); it != points.end(); it++ )
        {
          PPoint& p = *it;
          for ( size_t i = 0; i < p.reflections.size(); i++ )
              ret += it->charge / (pos - p.reflections[i].pos).length();
        }
    }

  if ( pc_q != 0 )
    {
      prec_t rr = (pos - pc_pos).magnitude();
      if ( rr < pc_rr )
        {
          ret += pc_q / pc_radius;
          ret += 4.0 * PREC_PI * pc_rho * (pc_rr - rr) / 6.0;
        }
      else
        {
          ret += pc_q / prec_t_sqrt (rr);
        }
    }

  ret *= PCONST;
#if 0
  char b[128];
  if ( !(ret > 0.1 && ret < 10000) )
  printf ("%s: %g\n", pos.sprint(b), ret);
#endif
  return ret;
}

PLink& PField::connect (PPoint& a, PPoint& b)
{
#if 1
  for ( links_t::iterator it = links.begin(); it != links.end(); it++ )
      assert ( !(it->a == &a && it->b == &b || it->a == &b && it->b == &a) );
  links.push_back (PLink (&a, &b));
  a.links.push_back (&links.back());
  b.links.push_back (&links.back());
  return links.back();
#else
  PPoint& m = addPoint (0.5 * (a.pos + b.pos));
  links.push_back (PLink (&a, &m));
  a.links.push_back (&links.back());
  m.links.push_back (&links.back());
  links.push_back (PLink (&m, &b));
  m.links.push_back (&links.back());
  b.links.push_back (&links.back());
#endif
}

PLink& PField::unsafe_connect (PPoint& a, PPoint& b)
{
  links.push_back (PLink (&a, &b));
  a.links.push_back (&links.back());
  b.links.push_back (&links.back());
  return links.back();
}

PLink* PField::safe_connect (PPoint& a, PPoint& b, const std::vector<PLink*>& check)
{
  if ( &a == &b ) return 0;
  for ( size_t i = 0; i < check.size(); i++ )
    {
      PLink& c = *check[i];
      if ( c.a == &a && c.b == &b || c.a == &b && c.b == &a )
          return 0;
    }
  links.push_back (PLink (&a, &b));
  a.links.push_back (&links.back());
  b.links.push_back (&links.back());
  return &links.back();
}

PLink& PField::split (PLink& link, PPoint& mid)
{
  PPoint& a = *link.a;
  PPoint& b = *link.b;
  link = PLink (&a, &mid);
  mid.links.push_back (&link);
  b.links.remove (&link);
  return unsafe_connect (mid, b);
}

bool PField::setTransforms (const std::vector<vect3d>& planes, const std::vector<transf3d>& transforms)
{
  if ( method != LINESYSTEM && method != POINTSYSTEM || transforms.empty() )
      return false;
  this->planes = planes;
  this->transforms = transforms;
  return true;
}

namespace
{
  struct tri_t
  {
    tri_t (PLink *l1, PLink *l2, PLink *l3) : l1(l1), l2(l2), l3(l3)
    {
      if ( this->l1 > this->l2 ) std::swap (this->l1, this->l2);
      if ( this->l2 > this->l3 ) std::swap (this->l2, this->l3);
      if ( this->l1 > this->l2 ) std::swap (this->l1, this->l2);
    }

    PLink *l1;
    PLink *l2;
    PLink *l3;

    friend bool operator< (const tri_t& l, const tri_t& r)
    {
      return l.l1 < r.l1 ||
          l.l1 == r.l1 && l.l2 < r.l2 ||
          l.l1 == r.l1 && l.l2 == r.l2 && l.l3 < r.l3;
    }
  };
};

void PField::split ()
{
  std::set<PPoint*> allpoints;
  std::set<PLink*> alllinks;

  // at first find all the triangles
  std::set<tri_t> triangles;
  for ( points_t::iterator it = points.begin(); it != points.end(); it++ )
    {
      PPoint *a = &*it;
      for ( PPoint::links_t::iterator alit = a->links.begin(); alit != a->links.end(); alit++ )
        {
          PLink *l1 = *alit;
          PPoint *b = l1->a == a ? l1->b : l1->a;
          for ( PPoint::links_t::iterator blit = b->links.begin(); blit != b->links.end(); blit++ )
            {
              PLink *l2 = *blit;
              if ( l1 == l2 ) continue;
              PPoint *c = l2->a == b ? l2->b : l2->a;
              for ( PPoint::links_t::iterator clit = c->links.begin(); clit != c->links.end(); clit++ )
                {
                  PLink *l3 = *clit;
                  if ( l2 == l3 ) continue;
                  PPoint *d = l3->a == c ? l3->b : l3->a;
                  if ( d != a ) continue;
                  triangles.insert (tri_t(l1, l2, l3));
                  allpoints.insert (a);
                  allpoints.insert (b);
                  allpoints.insert (c);
                  alllinks.insert (l1);
                  alllinks.insert (l2);
                  alllinks.insert (l3);
                }
            }
        }
    }

  // now find old total charge
  prec_t oldq = 0;
  if ( method == LINESYSTEM )
    {
      for ( std::set<PLink*>::iterator it = alllinks.begin(); it != alllinks.end(); it++ )
        {
          PLink& c = **it;
          oldq += c.reflections.size() * (c.len * c.a->rho + 0.5 * c.len * (c.b->rho - c.a->rho));
        }
    }
  else
    {
      for ( std::set<PPoint*>::iterator it = allpoints.begin(); it != allpoints.end(); it++ )
          oldq += (*it)->reflections.size() * (*it)->charge;
    }

  printf ("triangles.size=%zu, allpoints.size=%zu, allinks.size=%zu, oldq=%g\n",
      triangles.size(), allpoints.size(), alllinks.size(), prec2double(oldq));

  // now split up links and form new triangles
  for ( std::set<tri_t>::iterator it = triangles.begin(); it != triangles.end(); it++ )
    {
      PLink *links[3] = {it->l1, it->l2, it->l3};
      for ( size_t i = 0; i < 3; i++ )
        {
          PLink *l = links[i];
          if ( !l->tmp )
            {
              l->tmp = &addPoint (0.5 * (l->a->pos + l->b->pos),
                  0.5 * (l->a->pot_pos + l->b->pot_pos),
                  0.5 * (l->a->potential + l->b->potential));
              if ( l->boundary )
                  l->tmp->flags |= PPoint::IS_BOUNDARY;
              l->tmp->charge = 0.5 * (l->a->charge + l->b->charge);
              l->tmp->rho = 0.5 * (l->a->rho + l->b->rho);
              if ( l->a->flags & PPoint::NO_CDIST && l->b->flags & PPoint::NO_CDIST )
                  l->tmp->flags |= PPoint::NO_CDIST;
              allpoints.insert (l->tmp);
            }
        }
      alllinks.insert (&unsafe_connect (*links[0]->tmp, *links[1]->tmp));
      alllinks.insert (&unsafe_connect (*links[0]->tmp, *links[2]->tmp));
      alllinks.insert (&unsafe_connect (*links[1]->tmp, *links[2]->tmp));
    }

  for ( std::set<tri_t>::iterator it = triangles.begin(); it != triangles.end(); it++ )
    {
      PLink *links[3] = {it->l1, it->l2, it->l3};
      for ( size_t i = 0; i < 3; i++ )
        {
          PLink *l = links[i];
          if ( l->tmp )
            {
              alllinks.insert (&split (*l, *l->tmp));
              l->tmp = 0;
            }
        }
    }

  // re-reflect coordinate system
  reflect ();

  // calculate new charge
  prec_t newq = 0;
  if ( method == LINESYSTEM )
    {
      for ( std::set<PLink*>::iterator it = alllinks.begin(); it != alllinks.end(); it++ )
        {
          PLink& c = **it;
          newq += c.reflections.size() * (c.len * c.a->rho + 0.5 * c.len * (c.b->rho - c.a->rho));
        }
    }
  else
    {
      for ( std::set<PPoint*>::iterator it = allpoints.begin(); it != allpoints.end(); it++ )
          newq += (*it)->reflections.size() * (*it)->charge;
    }

  printf ("allpoints.size=%zu, allinks.size=%zu, newq=%g\n", allpoints.size(), alllinks.size(), prec2double(newq));

  // calculate and apply charge correction factor
  prec_t qf = oldq / newq;
  for ( std::set<PPoint*>::iterator it = allpoints.begin(); it != allpoints.end(); it++ )
    {
      if ( method == LINESYSTEM )
        {
          (*it)->rho *= qf;
        }
      else
        {
          (*it)->charge *= qf;
        }
    }

  // recalculate the rhodiff_by_llen for the links
  prec_t fq = 0;
  if ( method == LINESYSTEM )
    {
      for ( std::set<PLink*>::iterator it = alllinks.begin(); it != alllinks.end(); it++ )
        {
          PLink& c = **it;
          c.rhodiff_by_llen = (c.b->rho - c.a->rho) / c.len;
          fq += c.reflections.size() * (c.len * c.a->rho + 0.5 * c.len * (c.b->rho - c.a->rho));
        }
    }
  else
    {
      for ( std::set<PPoint*>::iterator it = allpoints.begin(); it != allpoints.end(); it++ )
          fq += (*it)->reflections.size() * (*it)->charge;
    }

  printf ("final.q=%g\n", prec2double(fq));

  // all done!
}

bool PField::fill (PMap& pmap)
{
  if ( method == LINESYSTEM )
      return false;

  for ( points_t::iterator it = points.begin(); it != points.end(); it++ )
    {
      PPoint& p = *it;
      for ( size_t i = 0; i < p.reflections.size(); i++ )
          pmap.addQ (it->reflections[i].pos, vect3d(0, 0, 0), it->charge);
    }
  if ( pc_q != 0 )
      pmap.addUniformSphere (pc_pos, pc_radius, pc_q);
  return true;
}

PPoint::PPoint (const vect3d& pos, const vect3d& pot_pos, prec_t potential) :
    pos(pos), pot_pos(pot_pos), potential(potential), charge(0), rho(0), inc(0)
{
  cur_efield.clear();
  flags = 0;
}

void StaticPotentialField::addFields (const vect3d& pos, vect3d& bfield, vect3d& efield)
{
  vect3d ef;
  pfield.getEfield (pos, ef);
  efield += ef;
}

void StaticPotentialField::addPotential (const vect3d& pos, prec_t& potential)
{
  potential += pfield.getPotential (pos);
}

void StaticPotentialField::draw(Screen& screen, const Screen::color& c)
{
}

std::string StaticPotentialField::get_povray_script ()
{
  return pfield.getPovrayScript();
}

bool StaticPotentialField::inContact (const vect3d& pos)
{
  prec_t mag = (pfield.resolution / 2 ) * (pfield.resolution / 2);
  prec_t mag2 = (pfield.resolution / 4 ) * (pfield.resolution / 4);

  if ( pfield.method == PField::LINESYSTEM )
    {
      PField::links_t& links = pfield.links;
      for ( PField::links_t::iterator it = links.begin(); it != links.end(); it++ )
        {
          PLink& c = *it;
          for ( size_t i = 0; i < c.reflections.size(); i++ )
            {
              vect3d p0 = c.reflections[i].a;
              vect3d p1 = c.reflections[i].b;
              vect3d ln = c.reflections[i].ab_normal;

              vect3d r0 = pos - p0;
              prec_t r0r0 = r0 * r0;
              prec_t ld = ln * r0;
              if ( r0r0 - ld * ld > mag2  )
                  continue;
              if ( ld >= 0 && ld <= c.len )
                  return true;
              if ( r0r0 <= mag2 || (p1 - pos).magnitude() <= mag2 )
                  return true;
            }
        }
      return false;
    }

  for ( PField::points_t::iterator it = pfield.points.begin(); it != pfield.points.end(); it++ )
    {
      PPoint& p = *it;
      for ( size_t i = 0; i < p.reflections.size(); i++ )
          if ( (p.reflections[i].pos - pos).magnitude() <= mag )
              return true;
    }

  if ( pfield.pc_q != 0 && pfield.pc_radius == 0 )
    {
      prec_t mag2 = (pfield.resolution / 2) * (pfield.resolution / 2);
      mag2 = 0.05 * 0.05;
      if ( (pos - pfield.pc_pos).magnitude() <= mag2 )
          return true;
    }
  return false;
}

